Robot Arm nco MINOS AMPL short = -L, <= L; # position var lon {Np} >= -pi, <= pi; # position var lat {Np} >= -pi/2, <= pi/2; # position var T >=0; # total time # velocities var rho_dot {i in Nv} = n*(rho[i+0.5] - rho[i-0.5])/T; var lon_dot {i in Nv} = n*(lon[i+0.5] - lon[i-0.5])/T; var lat_dot {i in Nv} = n*(lat[i+0.5] - lat[i-0.5])/T; # accelerations var rho_dot2 {i in Na} = n*(rho_dot[i+0.5] - rho_dot[i-0.5])/T; var lon_dot2 {i in Na} = n*(lon_dot[i+0.5] - lon_dot[i-0.5])/T; var lat_dot2 {i in Na} = n*(lat_dot[i+0.5] - lat_dot[i-0.5])/T; # lengths and moments of inertia var s {i in Na} = rho[i] + m_L*L/m_LB; var a {i in Na} # = I_3_23 - I_2_23 + m_B*rho[i]^2 + m_L*(rho[i] + L)^2; = ((L-rho[i])^3 + (L+rho[i])^3) * m_B / (3*2*L); var a_1 {i in Na} # = I_3_1 + I_2_23 + a[i]*cos(lat[i])^2; = ((L-rho[i])^3 + (L+rho[i])^3) * cos(lat[i])^2 * m_B / (3*2*L); var a_2 {i in Na} # = I_1_23 + m_B*rho[i]^2 + m_L*(rho[i] + L)^2; = ((L-rho[i])^3 + (L+rho[i])^3) * m_B / (3*2*L); minimize time: T; subject to F_bnds {i in Na}: -max_F <= m_LB * ( rho_dot2[i] - s[i]* ( ((lon_dot[i-0.5]+lon_dot[i+0.5])^2)/4 * cos(lat[i])^2 + ((lat_dot[i-0.5]+lat_dot[i+0.5])^2)/4 ) + g * sin(lat[i]) ) <= max_F; subject to T_lon_bnds {i in Na}: -max_T_lon <= a_1[i] * lon_dot2[i] - 2 * a[i] * sin(lat[i]) * cos(lat[i]) * (lon_dot[i-0.5]+lon_dot[i+0.5]) * (lat_dot[i-0.5]+lat_dot[i+0.5])/4 + 2 * m_LB * s[i] * cos(lat[i])^2 * (rho_dot[i-0.5]+rho_dot[i+0.5]) * (lon_dot[i-0.5]+lon_dot[i+0.5])/4 <= max_T_lon; subject to T_lat_bnds {i in Na}: -max_T_lat <= a_2[i] * lat_dot2[i] + a[i] * sin(lat[i]) * cos(lat[i]) * ((lon_dot[i-0.5]+lon_dot[i+0.5])^2)/4 + 2 * m_LB * s[i] * (rho_dot[i-0.5]+rho_dot[i+0.5]) * (lat_dot[i-0.5]+lat_dot[i+0.5])/4 + m_LB * g * s[i] * cos(lat[i]) <= max_T_lat; subject to init_rho: rho[0] = rho_0; subject to init_lon: lon[0] = lon_0; subject to init_lat: lat[0] = lat_0; subject to finl_rho: rho[n] = rho_n; subject to finl_lon: lon[n] = lon_n; subject to finl_lat: lat[n] = lat_n; subject to init_rho_dot: rho_dot[0.5] = 0; subject to init_lon_dot: lon_dot[0.5] = 0; subject to init_lat_dot: lat_dot[0.5] = 0; subject to finl_rho_dot: rho_dot[n-0.5] = 0; subject to finl_lon_dot: lon_dot[n-0.5] = 0; subject to finl_lat_dot: lat_dot[n-0.5] = 0; ]]> let n := 50; let L := 1.18; #:= 0.75; # half-length of arm let m_L := 0; # mass of load let m_B := 40; # mass of bar (unloaded arm) let I_1_23 := 18.5; # moment of inertia of bar about hor axis let I_2_23 := 0.0; #:= 0.12; # let I_3_23 := 18.5; # let I_3_1 := 0.0; # moment of inertia of base about base axis let max_F := 5; let max_T_lon := 300; let max_T_lat := 300; let rho_0 := 0.4*L; let lon_0 := 0; let lat_0 := pi/4; let rho_n := 0.4*L; let lon_n := 2*pi/3; let lat_n := pi/4; ####################################################################### # In order to get convergence, it seems to be necessary to initialize # to something sort of reasonable: let {i in Np} rho[i] := if (i in 1..n/2) then rho_0 + 2*(rho_n-rho_0)*(i-1)^2/(n-2)^2 else rho_n + 2*(rho_0-rho_n)*(i-n+1)^2/(n-2)^2; let {i in Np} lon[i] := if (i in 1..n/2) then lon_0 + 2*(lon_n-lon_0)*(i-1)^2/(n-2)^2 else lon_n + 2*(lon_0-lon_n)*(i-n+1)^2/(n-2)^2; let {i in Np} lat[i] := if (i in 1..n/2) then lat_0 + 2*(lat_n-lat_0)*(i-1)^2/(n-2)^2 else lat_n + 2*(lat_0-lat_n)*(i-n+1)^2/(n-2)^2; let rho[0] := rho_0; let lon[0] := lon_0; let lat[0] := lat_0; let rho[n] := rho_n; let lon[n] := lon_n; let lat[n] := lat_n; ####################################################################### fix T := 2.26; display T; option minos_options 'outlev=2 timing=1 superbasic_limit=500 \ major_iter=1000 minor_iter=9000'; solve; This problem is based on robotarm.mod available from http://www.princeton.edu/~rvdb/